# Description:  This controller gives to its robot the following behavior:
#               According to the messages it receives, the robot change its
#               behavior.

from controller import DifferentialWheels,Camera,DistanceSensor,Receiver

class Enumerate(object):
  def __init__(self, names):
    for number, name in enumerate(names.split()):
      setattr(self, name, number)

class Slave (DifferentialWheels):

  Mode = Enumerate('STOP MOVE_FORWARD AVOIDOBSTACLES TURN')
  timeStep = 32
  maxSpeed = 100.0
  mode = Mode.AVOIDOBSTACLES
  distanceSensors = []

  def boundSpeed(self, speed):
    return max(-self.maxSpeed, min(self.maxSpeed, speed))

  def initialization(self):
    self.mode = self.Mode.AVOIDOBSTACLES
    self.camera = self.getCamera('camera')
    self.camera.enable(4*self.timeStep)
    self.receiver = self.getReceiver('receiver')
    self.receiver.enable(self.timeStep)
    for dsnumber in range(0,2):
      self.distanceSensors[len(self.distanceSensors):]=[self.getDistanceSensor('ds'+str(dsnumber))]
      self.distanceSensors[len(self.distanceSensors)-1].enable(self.timeStep)

  def run(self) :
    while True :

      # Read sensors, particularly the order of the supervisor
      if self.receiver.getQueueLength()>0 :
        message = self.receiver.getData()
        self.receiver.nextPacket()
        print 'I should '+message+'!'
        if message == 'avoid obstacles':
          self.mode = self.Mode.AVOIDOBSTACLES
        elif message == 'move forward':
          self.mode = self.Mode.MOVE_FORWARD
        elif message == 'stop':
          self.mode = self.Mode.STOP
        elif message == 'turn':
          self.mode = self.Mode.TURN
      delta = self.distanceSensors[0].getValue()-self.distanceSensors[1].getValue()
      speeds = [0.0, 0.0]

      # send actuators commands according to the mode
      if self.mode == self.Mode.AVOIDOBSTACLES:
        speeds[0] = self.boundSpeed(self.maxSpeed/2+delta)
        speeds[1] = self.boundSpeed(self.maxSpeed/2-delta)
      elif self.mode == self.Mode.MOVE_FORWARD:
        speeds[0] = self.maxSpeed
        speeds[1] = self.maxSpeed
      elif self.mode == self.Mode.TURN:
        speeds[0] =  self.maxSpeed/2
        speeds[1] = -self.maxSpeed/2
      self.setSpeed(speeds[0], speeds[1])

      # perform a simulation step, leave the loop when
      # the controller has been killed
      if self.step(self.timeStep) == -1: break

controller = Slave()
controller.initialization()
controller.run()
